#include <ros/ros.h>
#include <tf/transform_listener.h>
#include "crazyflie_driver/Hover.h"
#include "crazyflie_driver/Position.h"
#include<uav/crazy_ecbf.h>
#include <std_srvs/Empty.h>
#include <std_msgs/String.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float32MultiArray.h>
#include <geometry_msgs/Twist.h>
#include "geometry_msgs/Vector3.h"
#include<sensor_msgs/Joy.h>
#include<nav_msgs/Path.h>
#include<geometry_msgs/PointStamped.h>
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <sensor_msgs/Joy.h>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <boost/thread.hpp>
#include <vector>
#include <string>
#include <iostream>
#include <sstream>
#include <math.h>
#include <std_msgs/Bool.h>

using namespace Eigen;
using namespace std;
#define pi 3.1415926535
double h=0.65;

sensor_msgs::Joy joy_info;
Eigen::Vector3d acc_desired,acc_ecbf;
ros::Time current_time;
nav_msgs::Path path;
geometry_msgs::PointStamped this_point_stamped;
int count_all=0;
int count_out=0;

int testPos(geometry_msgs::PoseStamped pos);
Eigen::Vector3d optimization(Eigen::Vector3d input,geometry_msgs::PoseStamped pose_now,geometry_msgs::PoseStamped pose_last,double dt);

class Crazyflie
{
public:
    Crazyflie():
    m_pose(),
    initial_pose(),
    deviation_pose(MatrixXf(2,1)),
    actual_pose(MatrixXf(2,1)),
    actual_height(0),
    flag(1){
        deviation_pose<<0,0;
        actual_pose<<0,0;
        // cout<<"Crazyflie construct"<<endl;
    }
    
    geometry_msgs::PoseStamped m_pose,last_pose; //测量位置
    geometry_msgs::PoseStamped initial_pose; //初始位置
    Eigen::Vector3d last_twt;

    MatrixXf deviation_pose; //摆放偏移量
    MatrixXf actual_pose; //实际位置
    double actual_height; //实际高度
    int flag;
};

class Controller
{
public:
    Controller(vector<double> deviation_pose_list):
    t(0.0),
    //ka(10),
    //questioning
    m_subscribePose(vector<ros::Subscriber>(2)),
    m_publishHover(vector<ros::Publisher>(2)),
    m_publishVel(vector<ros::Publisher>(2)),
    m_publishPos(vector<ros::Publisher>(2)),
    crazyflie(vector<Crazyflie>(2)),
    m_state(Idle), 
    m_serviceTakeoff(),
    m_serviceLand(),
    m_puberror_x(),
    m_puberror_y(),
    m_puberror_z(),
    m_puberror_ax(),
    m_puberror_ay(),
    m_puberror_az(),
   
    
    desired_position_0(MatrixXf(2,1)),
   // desired_position_1(2,Matrix<float, 2, 1>(-1.56375756836,0.239605026245)),
    desired_velocity_xy(MatrixXf(2,1)),
    desired_position_1(MatrixXf(2,1)),
   
    acceleration(2,Matrix<float, 2, 1>(0,0)),
    acceleration_z(0.0),
    desired_position_z(0.0),
    velocity_z(0.0),
   

    velocity(2,Matrix<float, 2, 1>(0,0)),
    
    algorithm_enable(0)

    {
        // cout<<"controller1"<<endl;
        ros::NodeHandle n;
        m_subscribePose= n.subscribe("/crazyflie1/vicon_pose", 10, &Controller::pose1_cb,this);
        //m_subscribePose[1] = n.subscribe("/crazyflie2/vicon_pose", 10, &Controller::pose2_cb,this);
       
        m_subscribe_algorithm_enable = n.subscribe("algorithm_enable", 10, &Controller::algorithm_enable_cb,this);
        m_subscribe_joy = n.subscribe("joy", 10, &Controller::joy_cb,this);

            m_publishHover= n.advertise<crazyflie_driver::Hover>("/crazyflie1/cmd_hover", 1);
            m_publishVel = n.advertise<geometry_msgs::Twist>("/crazyflie1/cmd_vel", 1);
            m_publishPos = n.advertise< crazyflie_driver::Position>("/crazyflie1/cmd_position", 1);


        m_serviceTakeoff = n.advertiseService("/crazyflie1/takeoff", &Controller::takeoff, this);
        m_serviceLand = n.advertiseService("/crazyflie1/land", &Controller::land, this);
        m_puberror_x= n.advertise<std_msgs::Float32>("/crazyflie/error_x", 1);
        m_puberror_y= n.advertise<std_msgs::Float32>("/crazyflie/error_y", 1);
        m_puberror_z= n.advertise<std_msgs::Float32>("/crazyflie/error_z", 1);

        m_puberror_ax= n.advertise<std_msgs::Float32>("/crazyflie/a_x", 1);
        m_puberror_ay= n.advertise<std_msgs::Float32>("/crazyflie/a_y", 1);
        m_puberror_az= n.advertise<std_msgs::Float32>("/crazyflie/a_z", 1);

        path_pub=n.advertise<nav_msgs::Path>("trajectory", 10);
        point_pub = nh.advertise<geometry_msgs::PointStamped>("point", 10);



            crazyflie.deviation_pose(0,0) = deviation_pose_list[0];
            crazyflie.deviation_pose(1,0) = deviation_pose_list[1];

    }

    void run(double frequency)
    {
        ros::NodeHandle node;
        // cout<<"run"<<endl;
        ros::Timer timer = node.createTimer(ros::Duration(1.0/frequency), &Controller::iteration, this);

        ros::spin();
    }

    


private:
    void pose1_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
        if(crazyflie.flag){
            crazyflie.initial_pose = *msg;
            crazyflie.flag = 0;
        }

        crazyflie.m_pose = *msg;
        this_point_stamped.point.x=crazyflie.m_pose.pose.position.x;
        this_point_stamped.point.y=crazyflie.m_pose.pose.position.y;
        this_point_stamped.point.z=crazyflie.m_pose.pose.position.z;

        point_pub.publish(this_point_stamped);

        geometry_msgs::PoseStamped this_pose_stamped;
        this_pose_stamped=crazyflie.m_pose;
        this_pose_stamped.header.stamp = ros::Time();
        this_pose_stamped.header.frame_id = "map";
        path.poses.push_back(this_pose_stamped);

        path_pub.publish(path);

        crazyflie.actual_pose(0,0) = crazyflie.m_pose.pose.position.x; //- crazyflie[0].initial_pose.pose.position.x + crazyflie[0].deviation_pose(0,0); 
        crazyflie.actual_pose(1,0) = crazyflie.m_pose.pose.position.y ;//- crazyflie[0].initial_pose.pose.position.y + crazyflie[0].deviation_pose(1,0); 
        crazyflie.actual_height = crazyflie.m_pose.pose.position.z ;//- crazyflie[0].initial_pose.pose.position.z; 
        //cout<<"crazyflie[0].actual_pose:"<<crazyflie[0].actual_pose<<endl;
    }

    /* void pose2_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
        if(crazyflie[1].flag){
            crazyflie[1].initial_pose = *msg;
            crazyflie[1].flag = 0;
        }
        crazyflie[1].m_pose = *msg;
        crazyflie[1].actual_pose(0,0) = crazyflie[1].m_pose.pose.position.x ;//- crazyflie[1].initial_pose.pose.position.x + crazyflie[1].deviation_pose(0,0); 
        crazyflie[1].actual_pose(1,0) = crazyflie[1].m_pose.pose.position.y; //- crazyflie[1].initial_pose.pose.position.y + crazyflie[1].deviation_pose(1,0); 
        crazyflie[1].actual_height = crazyflie[1].m_pose.pose.position.z ;
    } */




    void algorithm_enable_cb(const std_msgs::Bool::ConstPtr& msg){
        algorithm_enable = msg->data;
    }

    void joy_cb(const sensor_msgs::Joy::ConstPtr& msg){
        if(msg->buttons[5]){
            algorithm_enable = !algorithm_enable;
          //  cout<<algorithm_enable<<endl;
        }
        acc_desired[0]=msg->axes[0];
        acc_desired[1]=msg->axes[1];
        acc_desired[2]=msg->axes[2];
    }
   
    bool land(
        std_srvs::Empty::Request& req,
        std_srvs::Empty::Response& res)
    {
        ROS_INFO("Landing requested!");
        m_state = Landing;

        return true;
    }

    bool takeoff(
        std_srvs::Empty::Request& req,
        std_srvs::Empty::Response& res)
    {
        ROS_INFO("Takeoff requested!");
        m_state = TakingOff;
        return true;
    }


    void iteration(const ros::TimerEvent& e){
        // cout<<"hahaa"<<endl;
        switch(m_state)
        {
        case TakingOff:{
            //if(crazyflie[0].actual_height<0.1 | crazyflie[1].actual_height<0.1 | crazyflie[2].actual_height<0.1 | crazyflie[3].actual_height<0.1){
            
            if(crazyflie.actual_height<0.1 /* | crazyflie[1].actual_height<0.1 */){
             //if(crazyflie[1].actual_height<0.1){   
                crazyflie_driver::Hover msg_1;
                msg_1.vx = 0;
                msg_1.vy = 0;
                msg_1.yawrate = 0.0;
                msg_1.zDistance = h;
                m_publishHover.publish(msg_1);

/*                 crazyflie_driver::Hover msg_2;
                msg_2.vx = 0;
                msg_2.vy = 0;
                msg_2.yawrate = 0.0;
                msg_2.zDistance =h;
                m_publishHover[1].publish(msg_2); */

               // desired_position_1 << -1.56375756836,0.239605026245;
                
            }
            else{
                crazyflie.last_pose=crazyflie.m_pose;
                m_state = Automatic;
               
            }
            break;
        }
        case Automatic:{
            double frequency;
            frequency = 50.0;
            double k = 1.0;
            double dt(1./frequency);
            cout<<"t"<<t<<endl;

            if (algorithm_enable){
                t+=dt;

                cout<<"beginning"<<endl;
                geometry_msgs::PoseStamped pose_temp;
                pose_temp=crazyflie.m_pose;

                int flag=0;
                flag=testPos(crazyflie.m_pose);
                if(flag==3)
                {
                    ROS_INFO("within the range");
                }
                if(flag==4)
                {
                    ROS_INFO("out of range");
                    count_out++;
                }
                count_all++;
                ROS_INFO("detected for %d times, out for %d times",count_all,count_out);

                //acc_ecbf=optimization2(acc_desired,crazyflie[0].m_pose,last_twt,dt);
                acc_ecbf=optimization(acc_desired,crazyflie.m_pose,crazyflie.last_pose,dt);
                crazyflie.last_pose=pose_temp;

                acceleration<<acc_ecbf[0],acc_ecbf[1];
                acceleration_z=acc_ecbf[2];

                velocity+=acceleration*dt;
                velocity_z+=acceleration_z*dt;
                desired_position_z+=velocity_z*dt;

                last_twt[0]=velocity(0,0);
                last_twt[1]=velocity(1,0);
                last_twt[2]=velocity_z;


              

            }
    
           /*  else{
               // velocity[0] << 0,0;
                velocity[0] << 0,0;
                desired_position_z=h;
    
            } */


                geometry_msgs::Twist msg_4;
                msg_4.linear.x=velocity(0,0);
                msg_4.linear.y=velocity(1,0);
                msg_4.linear.z=velocity_z;
                m_publishVel.publish(msg_4);
           
            
            break;
        }


        case Landing:{
            if(crazyflie.actual_height>0.1 ){
                crazyflie_driver::Hover msg_L1;
                msg_L1.vx = 0;
                msg_L1.vy = 0;
                msg_L1.yawrate = 0;
                msg_L1.zDistance -= 0.01;

                m_publishHover.publish(msg_L1);

                cout<<"landing"<<endl;
                // }
            } else {
                m_state = Idle;
            }
            break;
        }

        case Idle:{
            geometry_msgs::Twist msg;
            
                m_publishVel.publish(msg);
            
            break;
        }
    }
    }

private:

    enum State
    {
        Idle = 0,
        Automatic = 1,
        TakingOff = 2,
        Landing = 3,
    };

private:
    ros::Subscriber m_subscribePose;
    ros::Publisher m_publishHover;
    ros::Publisher m_publishVel;
    ros::Publisher m_publishPos;

    ros::Publisher m_puberror_x;
    ros::Publisher m_puberror_y;
    ros::Publisher m_puberror_z;

    ros::Publisher m_puberror_ax;
    ros::Publisher m_puberror_ay;
    ros::Publisher m_puberror_az;

    ros::Publisher path_pub;
    ros::Publisher point_pub;
   
    ros::Subscriber m_subscribe_algorithm_enable;
    ros::Subscriber m_subscribe_joy;
 //   ros::Subscriber m_subscribe_extra_velocity;

    ros::ServiceServer m_serviceTakeoff;
    ros::ServiceServer m_serviceLand;

    Crazyflie crazyflie;
    
    State m_state;

    
    double t;
 //   double ka;
    bool algorithm_enable;

    

   
    //无人机期望位置
    MatrixXf desired_position_0;
    MatrixXf desired_position_1;
    MatrixXf desired_velocity_xy;
    
    

    
    Matrix<float, 2, 1>  acceleration;
    double acceleration_z;
    double desired_position_z;
    double velocity_z;
    Matrix<float, 2, 1>  velocity;
    Matrix<float, 2, 1>  quadrotor_v_expect;
    
 
};

int main(int argc, char **argv){
    ros::init(argc,argv,"mzl_ecbf");
    ros::NodeHandle n("~");
    double frequency;
    n.param("frequency", frequency, 50.0); 

    XmlRpc::XmlRpcValue param_list;
    vector<double> deviation_pose_list;
    if(!n.getParam("deviation_pose_list", param_list))
    ROS_ERROR("Failed to get parameter from server.");
    size_t elements_num = param_list.size();



    for (size_t i = 0; i < elements_num; ++i) 
    {
        XmlRpc::XmlRpcValue tmp_value = param_list[i];
        if(tmp_value.getType() == XmlRpc::XmlRpcValue::TypeDouble){
            deviation_pose_list.push_back(double(tmp_value));
        }
    }

    // cout<<"here"<<endl;
    Controller controller(deviation_pose_list);
    controller.run(frequency);
    return 0;
}